#include "height_model.h"

const Point HeightModel::offset(0.04f,0.0f);

HeightModel::HeightModel(){
	this->groundRef=0.5f;
}

HeightModel::~HeightModel(){
}

void HeightModel::setGroundRef(float groundRef){
	this->groundRef = groundRef;
}

float HeightModel::getGroundRef(){
	return this->groundRef;
}

void HeightModel::finalizeCurve(){
	/*CurveModel::Points::const_reverse_iterator rit=this->_curve->controlPointrBegin();
	Point p=*rit;
	Point p2=*(++rit);
	/Point pAux(p);
	pAux += Point(0.08f,0,0);
	this->_curve->movePoint(p2,p);
	this->_curve->movePoint(p,pAux);
	this->_curve->calculatePoints();*/
}

void HeightModel::movePoint(Point point,Point futurePoint){
	bool condPrev=true;
	bool condNext=true;
	Points::reverse_iterator itToPrev=std::find(this->control.rbegin(),this->control.rend(),point);
	Points::iterator itToNext=std::find(this->control.begin(),this->control.end(),point);

	if (itToPrev!=this->control.rend()){
		++itToPrev;
		if (*itToPrev==point)
			++itToPrev;
		if (itToPrev!=this->control.rend())
			condPrev=(*itToPrev).x1<futurePoint.x1;
		else
			condPrev=true;
	}

	if (itToNext!=this->control.end()){
		++itToNext;
		if( (itToNext!=this->control.end())&&((*itToNext)==point))
			++itToNext;
		if (itToNext!=this->control.end())
			condNext=(*itToNext).x1>futurePoint.x1;
		else
			condNext=true;
	}else
		condNext=true;
	if ((condPrev)&&(condNext)){
		CurveModel::movePoint(point,futurePoint);
		CurveModel::movePoint(point,futurePoint);
	}//else
		//CARTELLL !!!!!!!!!!
}

void HeightModel::removePoint(Point point){
	bool oneMore=false;
	bool isLast=false;
	Points::iterator it=std::find(this->control.begin(),this->control.end(),point);

	Points::iterator itToFirst=it;
	itToFirst++;
	if (itToFirst!=this->control.end())
		if(*itToFirst==point)
			oneMore=true;
		Points::iterator itToNextNext=itToFirst;
		itToNextNext++;
		if (itToNextNext!=this->control.end()){
			++itToNextNext;
			if (oneMore){
				++itToNextNext;
			}
	}else
		isLast=true;
	this->control.erase(itToFirst,itToNextNext);

	Points::reverse_iterator itToPrev=std::find(this->control.rbegin(),this->control.rend(),point);
	if (itToPrev!=this->control.rend()){
		++itToPrev;
		if (isLast)
			++itToPrev;
		if (itToPrev!=this->control.rend()){
	//		Points::iterator itToPrev2=reinterpret_cast<Points::iterator>(itToPrev);
			Points::iterator itToPrev2=std::find(this->control.begin(),this->control.end(),*itToPrev);
			Points::iterator itToDest=++itToPrev2;
			itToDest++;
			if (isLast)
				++itToDest;
			this->control.erase(itToPrev2,itToDest);
		}
	}
}

void HeightModel::addPoint(Point point){
	Point p1;
	if(!this->controlSize()){
		CurveModel::addPoint(point);
	}else{
		p1 = this->getLast();
		Point pPrev=point-HeightModel::offset;
		if (p1.x1>pPrev.x1)
			return;
		if(this->controlSize()>1)
			CurveModel::addPoint(p1);

		Point p2 = p1+offset;
		CurveModel::addPoint(p2);
		Point p3 = point-offset;
		CurveModel::addPoint(p3);
		CurveModel::addPoint(point);
	}
}
